#include "LeapTricks.h"


int LeapTricks::Init( int* argc, char** argv ) {
    // Init Leap //
    Leap::ImageList images;

    controller = new Leap::Controller();
    while( !controller->isConnected() ) {}

    controller->setPolicy( Leap::Controller::POLICY_BACKGROUND_FRAMES );
    controller->setPolicy( Leap::Controller::POLICY_IMAGES );

    do {
        images = controller->images();
    } while( !images[0].isValid() );

    // Init SFML //
    sf::RenderWindow window( sf::VideoMode( images[0].width()*2, images[0].height()*2 ), "LeapTricks", sf::Style::None );
    if ( !sf::Shader::isAvailable() ) {
        std::cout << "No shader support available" << std::endl;
        return 1;
    }

    // Create textures //
    sf::Uint8* buffer1 = new sf::Uint8[ images[0].width() * images[0].height() * 4 ];
    sf::Uint8* buffer2 = new sf::Uint8[ images[0].width() * images[0].height() * 4 ];
    sf::Uint8* distortBuffer = new sf::Uint8[ images[0].distortionHeight() * images[0].distortionHeight() * 4 ];

    sf::Texture tex1;
    if( !tex1.create( images[0].width(), images[0].height() ) ) {
        std::cout << "Failed to create texture" << std::endl;
        delete[] buffer1;
        delete[] buffer2;
        delete[] distortBuffer;
        return 1;
    }
    sf::Texture tex2( tex1 );
    sf::Texture distortTex1;
    if( !distortTex1.create( images[0].distortionHeight(), images[0].distortionHeight() ) ) {
        std::cout << "Failed to create texture" << std::endl;
        delete[] buffer1;
        delete[] buffer2;
        delete[] distortBuffer;
        return 1;
    }
    sf::Texture distortTex2( distortTex1 );

    for( int i=0; i<(images[0].distortionHeight()*images[0].distortionHeight()*2); i+=2 ) {
            if( images[0].distortion()[i] < 0.f ) {
                distortBuffer[i*2] = 0;
            } else if ( images[0].distortion()[i] > 1.f ) {
                distortBuffer[i*2] = 255;
            } else {
                distortBuffer[i*2] = images[0].distortion()[i] * 255;
            }

            if( images[0].distortion()[i+1] < 0.f ) {
                distortBuffer[i*2+1] = 0;
            } else if ( images[0].distortion()[i+1] > 1.f ) {
                distortBuffer[i*2+1] = 255;
            } else {
                distortBuffer[i*2+1] = images[0].distortion()[i+1] * 255;
            }
            distortBuffer[i*2+2] = distortBuffer[i*2+3] = 255;
    }
    distortTex1.update( distortBuffer );
    for( int i=0; i<(images[0].distortionHeight()*images[0].distortionHeight()*2); i+=2 ) {
            if( images[1].distortion()[i] < 0.f ) {
                distortBuffer[i*2] = 0;
            } else if ( images[1].distortion()[i] > 1.f ) {
                distortBuffer[i*2] = 255;
            } else {
                distortBuffer[i*2] = images[1].distortion()[i] * 255;
            }

            if( images[1].distortion()[i+1] < 0.f ) {
                distortBuffer[i*2+1] = 0;
            } else if ( images[1].distortion()[i+1] > 1.f ) {
                distortBuffer[i*2+1] = 255;
            } else {
                distortBuffer[i*2+1] = images[1].distortion()[i+1] * 255;
            }
            distortBuffer[i*2+2] = distortBuffer[i*2+3] = 255;
    }
    distortTex2.update( distortBuffer );

    sf::Sprite image1( tex1 );
    sf::Sprite image2( tex2 );

    image1.setScale( sf::Vector2f( 2.f, 2.f ) );
    image2.setPosition( sf::Vector2f( 0, images[0].height() ) );

    sf::Sprite test( distortTex1 );

    sf::Shader shader;
    if ( !shader.loadFromFile( "source/shader.vert", "source/shader.frag" ) ) {
        std::cout << "Failed to load fragment shader" << std::endl;
        delete[] buffer1;
        delete[] buffer2;
        delete[] distortBuffer;
        return 1;
    }

    // Main loop //
    while( window.isOpen() ) {
        sf::Event event;
        while( window.pollEvent( event ) ) {
            if ( event.type == sf::Event::Closed ) {
                window.close();
            } else if ( event.type == sf::Event::KeyPressed && event.key.code == sf::Keyboard::Escape ) {
                window.close();
            }
        }

        images = controller->images();

        for( int i=0; i<(images[0].width()*images[0].height()); ++i ) {
            buffer1[i*4] = buffer1[i*4+1] = buffer1[i*4+2] = images[0].data()[i];
            buffer1[i*4+3] = 255;

            buffer2[i*4] = buffer2[i*4+1] = buffer2[i*4+2] = images[1].data()[i];
            buffer2[i*4+3] = 255;
        }

        tex1.update( buffer1 );
        tex2.update( buffer2 );

        window.clear();
        shader.setParameter( "distortion", distortTex1 );
        shader.setParameter( "distortion2", distortTex2 );
        shader.setParameter( "texture2", tex2 );
        window.draw( image1, &shader );
        // shader.setParameter( "distortion", distortTex2 );
        // window.draw( image2, &shader );
//        window.draw( test );
        window.display();
    }

    delete[] buffer1;
    delete[] buffer2;
    delete[] distortBuffer;

    return 0;
}

LeapTricks::~LeapTricks() {

}
